from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import TimerAction


def generate_launch_description():
    px4_path = LaunchConfiguration('px4_path')
    ugv_ids = ['ugv1']
    px4_path_launch_arg = DeclareLaunchArgument(
        'px4_path',
        default_value= '/home/zsl/PX4-Autopilot',
        description='Path to PX4-Autopilot'
    )
    
    launch_dir = PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch'])
    return LaunchDescription([
        px4_path_launch_arg,

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'px4uav.launch.py']),
            launch_arguments={'uav_id': 'uav1', 'uav_pose': '0,0,0.5', 'uav_num': '0', 'px4_path': px4_path}.items()
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'gzpx.launch.py']),
            launch_arguments={'world_sdf': 'ugv_world.sdf','bridge_config':'one_ugv_bridge.yaml'}.items()
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch', 'gzugv.launch.py']),
            launch_arguments={
                'ugv_id': 'ugv1',
                'ugv_pose': '2.0,2.0,1.5,0,0,0'
            }.items()
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'microagent.launch.py'])
        ),
        

        TimerAction(
            period=3.0,  # 延迟时间，单位为秒
            actions=[
                IncludeLaunchDescription(
                    PathJoinSubstitution([launch_dir, 'uav.launch.py']),
                    launch_arguments={
                        'vehicleID': 'uav1',
                        'local_host_mode': 'False',
                        'multi_simulation_mode': 'True'
                    }.items()
                ),
            ]
        ),

        TimerAction(
            period=3.0,  # 延迟时间，单位为秒
            actions=[
                IncludeLaunchDescription(
                    PathJoinSubstitution([launch_dir, 'ugv.launch.py']),
                    launch_arguments={
                        'vehicleID': 'ugv1',
                        'local_host_mode': 'False',
                        'multi_simulation_mode': 'True',
                        'ugv_simulation_mode':'True'
                    }.items()
                )
            ]
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'ugv_allstart.launch.py']),
            launch_arguments={
                'ugv_ids': str(ugv_ids)  
            }.items()
        ),
        
        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'addi_ugv1_bridge.launch.py'])
        )

    ])